//
//  BZMatrix.c
//  Bezier
//
//  Created by Isabella Tromba on 12/15/11.
//  Copyright (c) 2011 __MyCompanyName__. All rights reserved.
//

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include "BZMatrix.h"

int factorial(int n)
{
	return (n == 1 || n == 0) ? 1 : factorial(n - 1) * n;
}

void BZReleaseMatrix(BZMatrix *mat)
{
    for (int i = 0; i < mat->rowCount; i++)
        free(mat->data[i]);
    free(mat->data);
}

void BZPrintMatrix(BZMatrix matrix, const char *matrixName)
{
	printf("%s\n",matrixName);
	for (int i = 0; i < matrix.rowCount; i++) {
		for (int j = 0; j < matrix.colCount; j++)
			printf("%f ", matrix.data[i][j]);
		printf("\n");
	}
}

BZMatrix BZCreateMatrix(int rowCount, int colCount)
{
    BZMatrix mat;
    mat.rowCount = rowCount;
    mat.colCount = colCount;
    
    mat.data = (double **)malloc(rowCount*sizeof(double *));
    for (int i = 0; i < rowCount; i++)
        mat.data[i] = (double *)malloc(colCount*sizeof(double));
    
    return mat;
}

BZMatrix BZMatrixCreateIdentity(int n){
	
	BZMatrix identityMatrix = BZCreateMatrix(n,n);
	for (int i = 0; i < n; i++)
		for (int j = 0; j < n; j++)
			identityMatrix.data[i][j] = (j == i) ? 1 : 0;
	
	return identityMatrix;
}

BZMatrix BZCopyMatrix(BZMatrix matrix)
{
    BZMatrix newMatrix = BZCreateMatrix(matrix.rowCount, matrix.colCount);
	
    for (int i = 0; i < matrix.rowCount; i++)
        memcpy(newMatrix.data[i], matrix.data[i], matrix.colCount*sizeof(double));
    
    return newMatrix;
}

void BZMatrixMultiply(BZMatrix matrixA, BZMatrix matrixB, BZMatrix *matrixOut)
{
    assert(matrixA.colCount == matrixB.rowCount);
    assert(matrixOut->rowCount == matrixA.rowCount && matrixOut->colCount == matrixB.colCount);
    
    for (int i = 0; i < matrixA.rowCount; i++)
        for (int j = 0; j < matrixB.colCount; j++)
            for (int k = 0; k < matrixB.rowCount; k++)
                matrixOut->data[i][j] += matrixA.data[i][k]*matrixB.data[k][j];
}

void BZMatrixColumnReplace(BZMatrix *destMatrix,  BZMatrix sourceMatrix, int colIndexDest, int colIndexSource)
{
	if (colIndexDest >= destMatrix->colCount) 
		return;
	if (colIndexSource >= sourceMatrix.colCount)
		return;
	if (destMatrix->rowCount != sourceMatrix.rowCount) 
		return;
	
	for (int i = 0; i < destMatrix->rowCount; i++)
		destMatrix->data[i][colIndexDest] = sourceMatrix.data[i][colIndexSource];
}

void BZMatrixScaleRow(BZMatrix *matrix, double scaleFactor, int row){
	for (int j = 0; j < matrix->colCount; j++)
		matrix->data[row][j] = matrix->data[row][j]*scaleFactor;	
}

void BZMatrixSubtractRow(BZMatrix *matrix, BZMatrix sourceMatrix, double scaleFactor,  int rowDest, int rowSource){
	
	assert(rowSource < sourceMatrix.rowCount);
	assert(rowDest < matrix->rowCount);
	assert(rowDest < matrix->rowCount);
	assert(sourceMatrix.rowCount == matrix->rowCount);
	
	for (int j = 0; j < sourceMatrix.colCount; j++)
		matrix->data[rowDest][j] += -sourceMatrix.data[rowSource][j]*scaleFactor;	
}

BZMatrix BZMatrixAugment(BZMatrix matrix1, BZMatrix matrix2)
{
	assert(matrix1.rowCount == matrix2.rowCount);
	
	BZMatrix augmentedMatrix = BZCreateMatrix(matrix1.rowCount, matrix1.colCount + matrix2.colCount);
	
	for (int i = 0; i < matrix1.rowCount; i++)
		for (int j = 0; j < matrix1.colCount; j ++)
			augmentedMatrix.data[i][j] = matrix1.data[i][j];
	
	for (int i = 0; i < matrix2.rowCount; i ++)
		for (int j = 0; j < matrix2.colCount; j ++)
			augmentedMatrix.data[i][j+matrix1.colCount] = matrix2.data[i][j];
	
	return augmentedMatrix;
}

BZMatrix BZMatrixSubMatrix(BZMatrix matrix, int rowStart, int colStart, int rowCount, int colCount)
{
	assert(rowStart + rowCount <= matrix.rowCount);
	assert(colStart + colCount <= matrix.colCount);
	assert(rowStart >= 0 && colStart >= 0);
	
	BZMatrix subMatrix = BZCreateMatrix(rowCount, colCount);
	
	for (int i = 0; i < subMatrix.rowCount; i++)
		for (int j = 0; j < subMatrix.colCount; j++)
			subMatrix.data[i][j] = matrix.data[i + rowStart][j + colStart];			
	
	return subMatrix;
}

BZMatrix BZMatrixInverse(BZMatrix matrix)
{
	BZMatrix inverseMatrix = BZMatrixCreateIdentity(matrix.rowCount);
	BZMatrix augmentedMatrix = BZMatrixAugment(matrix, inverseMatrix);
	
	for (int editingIndex = 0; editingIndex < matrix. rowCount; editingIndex ++)
		for (int i = 0; i < matrix.rowCount; i ++)
			if (editingIndex != i){
				double scaleFactor = augmentedMatrix.data[i][editingIndex]/augmentedMatrix.data[editingIndex][editingIndex];
				BZMatrixSubtractRow(&augmentedMatrix, augmentedMatrix, scaleFactor, i, editingIndex);
			}
	
	for (int i = 0; i < augmentedMatrix.rowCount; i++) {
		double scaleFactor = 1/augmentedMatrix.data[i][i];
		BZMatrixScaleRow(&augmentedMatrix, scaleFactor, i);
	}
	
	return BZMatrixSubMatrix(augmentedMatrix, 0, matrix.colCount, inverseMatrix.rowCount, inverseMatrix.colCount);
}

double BZMatrixDeterminant(BZMatrix matrix)
{
    double det = 0;
	if  (matrix.colCount == 1) 
		det = matrix.data[0][0];
	
    if (matrix.colCount == 2)
        det = matrix.data[0][0]*matrix.data[1][1] - matrix.data[0][1]*matrix.data[1][0];
    else 
        for(int i = 0; i < matrix.rowCount; i++)
		{
			BZMatrix subMatrix;
			subMatrix.rowCount = matrix.rowCount-1;
			subMatrix.colCount = matrix.colCount-1;
			subMatrix.data = (double **)malloc(subMatrix.rowCount*sizeof(double *));
			
			for(int j = 0; j < matrix.rowCount; j++)
			{
				if(j < i)
					subMatrix.data[j] = (double *)(matrix.data[j]+1);
			    if(j > i)
					subMatrix.data[j-1] = (double *)(matrix.data[j]+1);
			}
			
			det += powf(-1,i)*matrix.data[i][0]*BZMatrixDeterminant(subMatrix);        
		}
	
	return det;
}

void BZSolveLinearSystem(BZMatrix coeffMatrix, BZMatrix constMatrix, BZMatrix *solutionMatrix)
{
	BZMatrixMultiply(BZMatrixInverse(coeffMatrix), constMatrix, solutionMatrix);	
}



double BZBernstein(int i, int n, double t)
{
	return (double)factorial(n) / (double)(factorial(n-i)*factorial(i)) * powf(1.0f-t, n-i) * powf(t, i);
}

void BZInterpolateCurve(BZMatrix interpolantPointsX, BZMatrix interpolantPointsY, BZMatrix *controlPointsX, BZMatrix *controlPointsY)
{
	BZMatrix coeffMatrix = BZCreateMatrix(interpolantPointsX.rowCount, interpolantPointsY.rowCount);
	
	for (int i = 0; i < coeffMatrix.rowCount; i++)
		for (int j = 0; j < coeffMatrix.colCount; j++)
			coeffMatrix.data[i][j] = BZBernstein(j, interpolantPointsX.rowCount-1, (double)i / (double)(interpolantPointsX.rowCount-1));
    
	BZSolveLinearSystem(coeffMatrix, interpolantPointsX, controlPointsX);
	BZSolveLinearSystem(coeffMatrix, interpolantPointsY, controlPointsY);
}

void BZDrawCurve(BZMatrix controlPointsX, BZMatrix controlPointsY, CGContextRef myContext)
{
    
    double tempX,tempY;
    bezierTransformationsToView(controlPointsX.data[0][0],controlPointsY.data[0][0],&tempX, &tempY);
    CGContextMoveToPoint(myContext, tempX, tempY);
	BZMatrix newControlPointsX = BZCreateMatrix(controlPointsX.rowCount, 1);
	BZMatrix newControlPointsY = BZCreateMatrix(controlPointsY.rowCount, 1);
	
	int pointCount = controlPointsX.rowCount;
    
	for (float i = 0.0f; i <= 1.0f; i += .01f) {
		int newpointCount = pointCount;
        for (int j = 0; j < pointCount; j++)  {
            newControlPointsX.data[j][0] = controlPointsX.data[j][0];
			newControlPointsY.data[j][0] = controlPointsY.data[j][0];
        }
		while (newpointCount > 1) {
			for(int j = 0; j<newpointCount-1; j++) {
				newControlPointsX.data[j][0] = newControlPointsX.data[j][0] + i*newControlPointsX.data[j+1][0] - i*newControlPointsX.data[j][0];
				newControlPointsY.data[j][0] = newControlPointsY.data[j][0] + i*newControlPointsY.data[j+1][0] - i*newControlPointsY.data[j][0];
			}
			newpointCount += -1;
		}
        bezierTransformationsToView(newControlPointsX.data[0][0],newControlPointsY.data[0][0],&tempX, &tempY);
        printf("%lf %lf\n",newControlPointsX.data[0][0],newControlPointsY.data[0][0]);
		CGContextAddLineToPoint(myContext, tempX, tempY);
	}
	
	CGContextStrokePath(myContext);
}

void bezierTransformationsToView(double xl,double yl,double *x,double *y)
{
    double screenW = kScreenW;
    double screenH = kScreenH;
    double zoom = kZoom;
    
    xl = xl/zoom;
    yl = yl/zoom;
    *x = xl * (screenW/2.0) + screenW/2.0;
    *y = (screenH/2.0) - yl * (screenH/2.0);
    //return CGPointMake(x,y);
}
